import numpy as  np
import time

T = 280

def phase_shift(PulseGen):
	PulseGen.write(0x00, 0x01)
	time.sleep(0.001)
	PulseGen.write(0x00, 0x00)

def get_FPGA_temperature():
    fp = open("/sys/bus/iio/devices/iio:device0/in_temp2_pl_temp_raw")
    raw = int(fp.readline().replace('\n', ''))
    fp.close()
    return raw*509.314/65536.0-280.23

def get_CPU_temperature():
    fp = open("/sys/bus/iio/devices/iio:device0/in_temp0_ps_temp_raw")
    raw = int(fp.readline().replace('\n', ''))
    fp.close()
    return raw*509.314/65536.0-280.23

def tdm_pad(tdm_raw, R):
	T = tdm_raw.shape[0]
	tdm_padded = np.zeros(T+2*R, dtype=np.uint8)
	tdm_padded[0:R] = tdm_raw[T-R:T]
	tdm_padded[R:T+R] = tdm_raw
	tdm_padded[T+R:T+2*R] = tdm_raw[0:R]
	return tdm_padded

def filter_tdm(tdm_raw, R):
	T = tdm_raw.shape[0]
	tdm_padded = tdm_pad(tdm_raw, R)

	for i in range(0,T):
		if(tdm_padded[i+R]==1):
			if(tdm_padded[i:i+2*R+1].sum() == 1):
				tdm_filtered[i] = 0
			else:
				tdm_filtered[i] = 1
		else:
			if(tdm_padded[i:i+2*R+1].sum() == 2*R):
				tdm_filtered[i] = 1
			else:
				tdm_filtered[i] = 0
	return tdm_filtered

def find_theta(tdm_filtered):
	T = tdm_filtered.shape[0]
	tdm_padded = tdm_pad(tdm_filtered,1)
	theta1_cnt = 0
	theta2_cnt = 0
	for i in range(T):
		if(tdm_filtered[i-1]==0 and tdm_filtered[i]==1):
			theta1 = i
			theta1_cnt += 1
		if(tdm_filtered[i-1]==1 and tdm_filtered[i]==0):
			theta2 = i
			theta2_cnt += 1
	if(theta1_cnt != 1 or theta2_cnt != 1):
		print("find jitters")

	return theta1, theta2

def calculate_theta(theta1, theta2, T):
	if(theta1 == 0):
		theta = theta2
	elif(theta2 == 0):
		theta = theta1 + T/2
	elif(theta1 > theta2):
		theta = (theta1 + theta2 - T/2)/2
	else:
		theta = (theta1 + theta2 + T/2)/2
	return theta

def mff_h(tdm_raw, R):
	T = tdm_raw.shape[0]
	tdm_padded = tdm_pad(tdm_raw, R)
	tdm_mff_h = np.zeros(T, dtype=np.uint8)
	for i in range(0,T):
		tdm_mff_h[i] = tdm_padded[i:i+2*R+1].mean().round()
	return tdm_mff_h

def mff_v(tdm_raw, path_index, T):
	tdm_mff_v = np.zeros(T, dtype = np.uint8)
	for i in range(T):
		value = np.zeros(tdm_raw.shape[0], dtype = np.uint8)
		for r in range(tdm_raw.shape[0]):
			if(tdm_raw[r, i]& (1<<path_index)):
				value[r] = 1
			tdm_mff_v[i] = value.mean().round()
	return tdm_mff_v